﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.IO;
using MotionPlanning;

namespace MotionPlanning.MotionPlanningComponents.MotionPlanningSystems
{
    // System mathematical environment
    abstract public class RoboticSystemComponent
    {
        protected string System_Type;
        protected int DOF_state;
        protected int DOF_control;
        protected double[] q_max;
        protected double[] q_min;
        protected int[] q_topology;
        protected double[] q_scale;
        protected double[] ctrl_max;
        protected double[] ctrl_min;

        protected double[] k1;
        protected double[] k2;
        protected double[] k3;
        protected double[] k4;
        protected double[] Qd_out;
        protected double[] Qd_in;
        protected bool isIntegrationReady = false;
        protected double t_delta = 0.01;

        virtual public bool GoalReached(double[] q, double[] q_target, double tol)
        {
            throw new NotImplementedException();
        }
        virtual public void Calculate_q_alias(double[] q, ref double[] q_alias)
        {
            q_alias = q;
        }

        virtual public void Qd(double[] q, double[] ctrl, ref double[] qd){
            throw new NotImplementedException();
        }
        virtual public void Integration(double[] q, double[] ctrl, double t_delta, ref double[] q_next)
        {
            if (isIntegrationReady == false)
                PrepareForIntegration();

            Qd(q, ctrl, ref Qd_out);
            MotionPlanningUtils.vector_kTimes(Qd_out,t_delta,ref k1);

            MotionPlanningUtils.vector_vlinear(k1,0.5,q,ref Qd_in);
            Qd(Qd_in, ctrl, ref Qd_out);
            MotionPlanningUtils.vector_kTimes(Qd_out, t_delta, ref k2);

            MotionPlanningUtils.vector_vlinear(k2,0.5,q,ref Qd_in);
            Qd(Qd_in, ctrl, ref Qd_out);
            MotionPlanningUtils.vector_kTimes(Qd_out, t_delta, ref k3);

            MotionPlanningUtils.vector_vlinear(k3,1,q,ref Qd_in);
            Qd(Qd_in, ctrl, ref Qd_out);
            MotionPlanningUtils.vector_kTimes(Qd_out, t_delta, ref k4);

            MotionPlanningUtils.vector_vlinear(k1, 1.0 / 6.0, q, ref Qd_out);
            MotionPlanningUtils.vector_vlinear(k2, 1.0 / 3.0, Qd_out, ref Qd_in);
            MotionPlanningUtils.vector_vlinear(k3, 1.0 / 3.0, Qd_in, ref Qd_out);
            MotionPlanningUtils.vector_vlinear(k4, 1.0 / 6.0, Qd_out, ref q_next);
        }
        virtual public void Integration(RK4Helper pad, double[] q, double[] ctrl, double t_delta, ref double[] q_next)
        {
            if (pad == null)
                pad = new RK4Helper(q.Length);

            Qd(q, ctrl, ref pad.Qd_out);
            MotionPlanningUtils.vector_kTimes(pad.Qd_out, t_delta, ref pad.k1);

            MotionPlanningUtils.vector_vlinear(pad.k1, 0.5, q, ref pad.Qd_in);
            Qd(pad.Qd_in, ctrl, ref pad.Qd_out);
            MotionPlanningUtils.vector_kTimes(pad.Qd_out, t_delta, ref pad.k2);

            MotionPlanningUtils.vector_vlinear(pad.k2, 0.5, q, ref pad.Qd_in);
            Qd(pad.Qd_in, ctrl, ref pad.Qd_out);
            MotionPlanningUtils.vector_kTimes(pad.Qd_out, t_delta, ref pad.k3);

            MotionPlanningUtils.vector_vlinear(pad.k3, 1, q, ref pad.Qd_in);
            Qd(pad.Qd_in, ctrl, ref pad.Qd_out);
            MotionPlanningUtils.vector_kTimes(pad.Qd_out, t_delta, ref pad.k4);

            MotionPlanningUtils.vector_vlinear(pad.k1, 1.0 / 6.0, q, ref pad.Qd_out);
            MotionPlanningUtils.vector_vlinear(pad.k2, 1.0 / 3.0, pad.Qd_out, ref pad.Qd_in);
            MotionPlanningUtils.vector_vlinear(pad.k3, 1.0 / 3.0, pad.Qd_in, ref pad.Qd_out);
            MotionPlanningUtils.vector_vlinear(pad.k4, 1.0 / 6.0, pad.Qd_out, ref q_next);
        }

        public void PrepareForIntegration()
        {
            k1 = new double[DOF_state];
            k2 = new double[DOF_state];
            k3 = new double[DOF_state];
            k4 = new double[DOF_state];
            Qd_out = new double[DOF_state];
            Qd_in = new double[DOF_state];
            isIntegrationReady = true;
        }

        abstract public bool Steering(double[] q_start, double[] q_end, ref double[] u, ref double duration, ref double edgeCost);

        virtual public bool Propagate(double[] q_start, double[] u, double duration, ref double[] q_end, ref double step_cost)
        {
            int n_step = (int)Math.Floor(duration / t_delta + 0.5);
            double[] q_in = new double[q_start.Length];
            MotionPlanningUtils.vector_set(q_start, ref q_in);
            step_cost = 0;
            for (int i = 0; i < n_step; ++i)
            {
                this.Integration(q_in, u, t_delta, ref q_end);
                MotionPlanningUtils.vector_set(q_end, ref q_in);
                step_cost += t_delta;
            }
            return true;
        }

        virtual public bool Propagate(RK4Helper pad, double[] q_start, double[] u, double duration, ref double[] q_end, ref double step_cost)
        {
            int n_step = (int)Math.Floor(duration / t_delta + 0.5);
            double[] q_in = new double[q_start.Length];
            MotionPlanningUtils.vector_set(q_start, ref q_in);
            MotionPlanningUtils.vector_set(q_start, ref q_end);
            step_cost = 0;
            for (int i = 0; i < n_step; ++i)
            {
                this.Integration(pad, q_in, u, t_delta, ref q_end);
                MotionPlanningUtils.vector_set(q_end, ref q_in);
                step_cost += t_delta;
            }
            return true;
        }

        virtual public bool Propagate(double R_p, double[] q_start, double[] u, ref double[] q_end, ref double cost)
        {
            double propagate_distance = 0;
            int max_try = 100;
            int i_try = 0;
            double duration_step = 0.05;
            double step_cost = 0;
            double[] q_tmp = new double[q_start.Length];
            MotionPlanningUtils.vector_set(q_start, ref q_tmp);
            while(i_try < max_try && propagate_distance < R_p)
            {
                i_try++;

                bool success_step = Propagate(q_tmp, u, duration_step, ref q_end, ref step_cost);
                if (success_step == false) return false;
                cost += step_cost;
                MotionPlanningUtils.vector_set(q_end, ref q_tmp);

                propagate_distance = Math.Sqrt(Distance2_Topological(q_start, q_end));
            }
            return true;
        }

        virtual public bool Propagate_HighSpeed(double[] q_start, double[] u, double duration, ref double[] q_end, ref double step_cost)
        {
            return Propagate(q_start, u, duration, ref q_end, ref step_cost);
        }

        virtual public bool LoadingSystemParameters(string fileName)
        {
            TextReader tr = new StreamReader(fileName);
            if (tr == null)
                throw new Exception("input file cannot be opened.");
            string rawFile = tr.ReadToEnd();

            string[] separators = new string[] { "<RoboticSystem>", "</RoboticSystem>" };
            string[] entries = rawFile.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            string roboticSystemText = entries[1];

            separators = new string[] { "<System_Type>", "</System_Type>" };
            entries = roboticSystemText.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            this.System_Type = entries[1];

            separators = new string[] { "<DOF_state>", "</DOF_state>" };
            entries = roboticSystemText.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            this.DOF_state = int.Parse(entries[1]);

            separators = new string[] { "<DOF_control>", "</DOF_control>" };
            entries = roboticSystemText.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            this.DOF_control = int.Parse(entries[1]);

            separators = new string[] { "<q_max>", "</q_max>" };
            entries = roboticSystemText.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            this.q_max = MotionPlanningUtils.StringToDoubleArray(entries[1]);

            separators = new string[] { "<q_min>", "</q_min>" };
            entries = roboticSystemText.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            this.q_min = MotionPlanningUtils.StringToDoubleArray(entries[1]);

            separators = new string[] { "<ctrl_max>", "</ctrl_max>" };
            entries = roboticSystemText.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            this.ctrl_max = MotionPlanningUtils.StringToDoubleArray(entries[1]);

            separators = new string[] { "<ctrl_min>", "</ctrl_min>" };
            entries = roboticSystemText.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            this.ctrl_min = MotionPlanningUtils.StringToDoubleArray(entries[1]);

            separators = new string[] { "<q_scale>", "</q_scale>" };
            entries = roboticSystemText.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            this.q_scale = MotionPlanningUtils.StringToDoubleArray(entries[1]);

            separators = new string[] { "<q_topology>", "</q_topology>" };
            entries = roboticSystemText.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            this.q_topology = MotionPlanningUtils.StringToIntArray(entries[1]);

            tr.Close();
            return true;
        }
        
        virtual public double Distance2_Topological(double[] x1, double[] x2)
        {
            throw (new Exception("Unimplemented function - Distance_Topological()."));
        }

        public double Distance2_Euclidean(double[] x1, double[] x2)
        {
            double sum2 = 0;
            double diff_i = 0;
            for (int i = 0; i < DOF_state; ++i)
            {
                diff_i = (x1[i] - x2[i]) * q_scale[i];
                sum2 += diff_i * diff_i;
            }
            return sum2;
        }

        public bool PointInRange(double[] q)
        {
            for (int i = 0; i < DOF_state; ++i)
            {
                if (q_topology[i] != 1) continue;   //1 indicates euclidean
                if (q[i] < this.q_min[i]) return false;
                if (q[i] > this.q_max[i]) return false;
            }
            return true;
        }

        public int Get_DOF_state() { return DOF_state; }
        public int Get_DOF_control() { return DOF_control; }
        public double[] Get_q_max() { return q_max; }
        public double[] Get_q_min() { return q_min; }
        public int[] Get_q_topology() { return q_topology; }
        public double[] Get_q_scale() { return q_scale; }
        public double[] Get_ctrl_max() { return ctrl_max; }
        public double[] Get_ctrl_min() { return ctrl_min; }
        override public string ToString()
        {
            return "\n\t<System_Parameters>" +
                    "\n\t\t<DOF_state>" + DOF_state + "</DOF_state>" +
                    "\n\t\t<DOF_control>" + DOF_control + "</DOF_control>" +
                    "\n\t\t<q_max>" + MotionPlanningUtils.ArrayToString(q_max) + "</q_max>" +
                    "\n\t\t<q_min>" + MotionPlanningUtils.ArrayToString(q_min) + "</q_min>" +
                    "\n\t\t<q_topology>" + MotionPlanningUtils.ArrayToString(q_topology) + "</q_topology>" +
                    "\n\t\t<q_scale>" + MotionPlanningUtils.ArrayToString(q_scale) + "</q_scale>" +
                    "\n\t\t<ctrl_max>" + MotionPlanningUtils.ArrayToString(ctrl_max) + "</ctrl_max>" +
                    "\n\t\t<ctrl_min>" + MotionPlanningUtils.ArrayToString(ctrl_min) + "</ctrl_min>" +
                    "\n\t</System_Parameters>\n";
        }
    }
}
